
#include "libmri.h"

#include <omp.h>

#include <algorithm>
using std::max;
using std::min;

void
calib3d_atamul (cplxf *z, cplxf *calib, int *csize, cplxf *x, int *xsize)
{
	int cs1 = csize[0], cs2 = csize[1], cs3 = csize[2], nch = csize[3];
	int xs1 = xsize[0], xs2 = xsize[1], xs3 = xsize[2];

	int m1, m2, m3, m = (m1 = cs1-xs1+1)*(m2 = cs2-xs2+1)*(m3 = cs3-xs3+1);
	int n = xs1*xs2*xs3*nch;

	cplxf *y = new cplxf [m];

	#pragma omp parallel
	{
		#pragma omp for
		for (int i = 0; i < m; ++i) y[i] = 0.0f;

		for (int ch = 0; ch < nch; ++ch)
		{
			#pragma omp for
			for (int i3 = 0; i3 < m3; ++i3)
			  for (int i2 = 0; i2 < m2; ++i2)
				for (int i1 = 0; i1 < m1; ++i1)
				{
					for (int j3 = 0; j3 < min (xs3, cs3-i3); ++j3)
					  for (int j2 = 0; j2 < min (xs2, cs2-i2); ++j2)
						for (int j1 = 0; j1 < min (xs1, cs1-i1); ++j1)

						{
							int c1 = i1+j1, c2 = i2+j2, c3 = i3+j3;

							y[i1 + m1*(i2 + m2*i3)] +=
								calib [c1 + cs1*(c2 + cs2*(c3 + cs3*ch))]
								  * x [j1 + xs1*(j2 + xs2*(j3 + xs3*ch))];
						}

				}
		}

		#pragma omp for
		for (int i = 0; i < n; ++i) z[i] = 0.0f;

		#pragma omp for
		for (int ch = 0; ch < nch; ++ch)
		{
			for (int i3 = 0; i3 < xs3; ++i3)
			  for (int i2 = 0; i2 < xs2; ++i2)
				for (int i1 = 0; i1 < xs1; ++i1)
				{
					for (int j3 = 0; j3 < min (m3, cs3-i3); ++j3)
					  for (int j2 = 0; j2 < min (m2, cs2-i2); ++j2)
						for (int j1 = 0; j1 < min (m1, cs1-i1); ++j1) 
						{
							int c1 = i1+j1, c2 = i2+j2, c3 = i3+j3;

							z [i1 + xs1*(i2 + xs2*(i3 + xs3*ch))] += 
								conj(calib [c1 + cs1*(c2 + cs2*(c3 + cs3*ch))])
								       * y [j1 +  m1*(j2 +  m2*(j3))];
						}
				}
		}
	}

	delete [] y;
}

#ifdef _MAIN_

int main (int argc, char **argv)
{
	shm_array_t *shm_calib = 0, *shm_x = 0;

	struct arg *args[] = {
		new arg_t <shm_array_t*> ("calib", shm_calib),
		new arg_t <shm_array_t*> ("x", shm_x),
		0};

	get_args (argc, argv, args);

	if (shm_calib == 0){
		if (get_shm_array ("calib", &shm_calib)){
			perror("calib");
			return -1;
		}
	}
	cplxf *calib = (cplxf*) shm_calib->data.c;
	int cs1 = shm_calib->dims[0],
	    cs2 = shm_calib->dims[1],
	    cs3 = shm_calib->dims[2],
	    nch = shm_calib->dims[3];

	int csize[4] = {cs1, cs2, cs3, nch};

	if (shm_x == 0){
		if (get_shm_array ("x", &shm_x)){
			perror ("x");
			return -1;
		}
	}

	int xs1 = shm_x->dims[0],
	    xs2 = shm_x->dims[1],
	    xs3 = shm_x->dims[1];

	int xsize[3] = {xs1, xs2, xs3};
	cplxf *x = (cplxf*) shm_x->data.c;

	shm_array_t z_size, *shm_z = 0;
	z_size.type = shm_array_cplxf;
	z_size.ndims = 4;
	z_size.dims[0] = xs1;
	z_size.dims[1] = xs2;
	z_size.dims[2] = xs3;
	z_size.dims[3] = nch;

	if (make_shm_array ("z", &z_size, &shm_z)){
		perror("z");
		return -1;
	}
	cplxf *z = (cplxf*) shm_z->data.c;

	calib3d_atamul (z, calib, csize, x, xsize);

	return 0;
}
#endif
